package icommand.navigation;

import icommand.nxt.*;

/**
 * The TachoNavigator class can keep track of the robot position and the direction angle it faces;
 * It uses a _pilot object to control NXT robot movements.<br>
 * The position and direction angle values are updated automatically when the movement command
 * returns after the movement is complete and and after stop() command is issued. However, some
 * commands optionally return immediately, to permit sensor monitoring in the main thread. It is
 * then the programmers responsibility to call updatePosition() when the robot motion is completed.
 * All angles are in degrees, distances in the units used to specify robot dimensions. As with
 * pilot, the robot must be have two independently controlled drive wheels. The assumed initial
 * position of the robot is at (0,0) and initial angle 0 i.e. pointing in the +X direction.
 */

public class TachoNavigator implements Navigator {
  // orientation and co-ordinate data
  protected double _heading = 0;
  private double _x = 0;
  private double _y = 0;
  protected int _left0 = 0;
  protected int _right0 = 0;

  // The essential component
  protected Pilot _pilot;

  /**
   * set false whenever the robot moves, set to true by updatePosition();
   */
  private boolean _updated = false;

  /**
   * Allocates a Navigator object and initializes it with the proper motors. The x and y values and
   * the direction angle are all initialized to 0, so if the first move is forward() the robot will
   * run along the x axis. <BR>
   * 
   * @param wheelDiameter
   *          The diameter of the wheel, usually printed right on the wheel, in centimeters (e.g.
   *          49.6 mm = 4.96 cm = 1.95 in)
   * @param trackWidth
   *          The distance from the center of the left tire to the center of the right tire, in
   *          units of your choice
   * @param rightMotor
   *          The motor used to drive the right wheel e.g. Motor.C.
   * @param leftMotor
   *          The motor used to drive the left wheel e.g. Motor.A.
   * @param reverse
   *          If motor.forward() dives the robot backwars, set this parameter true.
   */
  public TachoNavigator(double wheelDiameter, double trackWidth, Motor leftMotor, Motor rightMotor,
      boolean reverse) {
    _pilot = new Pilot(wheelDiameter, trackWidth, leftMotor, rightMotor, reverse);
  }

  public TachoNavigator(double wheelDiameter, double trackWidth, Motor leftMotor, Motor rightMotor) {
    _pilot = new Pilot(wheelDiameter, trackWidth, leftMotor, rightMotor);
  }

  public TachoNavigator(Pilot pilot) {
    _pilot = pilot;
  }

  /**
   * Overloaded TachoNavigator constructor that assumes the following:<BR>
   * Left motor = Motor.A Right motor = Motor.C <BR>
   * 
   * @param wheelDiameter
   *          The diameter of the wheel, usually printed right on the wheel, in centimeters (e.g.
   *          49.6 mm = 4.96 cm)
   * @param driveLength
   *          The distance from the center of the left tire to the center of the right tire,
   */
  public TachoNavigator(double wheelDiameter, double driveLength) {
    this(wheelDiameter, driveLength, Motor.A, Motor.C);
  }

  public Pilot getPilot() {
    return _pilot;
  }

  /**
   * Returns the current x coordinate of the NXT.
   * 
   * @return double Present x coordinate.
   */
  public double getX() {
    return _x;
  }

  /**
   * Returns the current y coordinate of the NXT. Note: At present it will only give an updated
   * reading when the NXT is stopped.
   * 
   * @return double Present y coordinate.
   */
  public double getY() {
    return _y;
  }

  /**
   * Returns the current angle the NXT robot is facing, relative to the +X axis direction; the +Y
   * direction is 90 degrees. Note: At present it will only give an updated reading when the NXT is
   * stopped.
   * 
   * @return double Angle in degrees.
   */
  public double getAngle() {
    return _heading;
  }

  /**
   *sets robot location (x,y) and direction angle
   * 
   * @param x
   *          the x coordinate of the robot
   *@param y
   *          the y coordinate of the robot
   *@param directionAngle
   *          the angle the robot is heading, measured from the x axis. 90 degrees is the +Y
   *          direction
   */
  public void setPosition(double x, double y, double directionAngle) {
    _x = x;
    _y = y;
    _heading = directionAngle;
  }

  /**
   *sets the motor speed of the robot, in degrees/second.
   */
  public void setSpeed(int speed) {
    _pilot.setSpeed(speed);
  }

  /**
   * Moves the NXT robot forward until stop() is called.
   * 
   * @see Navigator#stop().
   */
  public void forward() {
    _updated = false;
    _pilot.resetTachoCount();
    _right0 = 0;
    _left0 = 0;
    _pilot.forward();
  }

  /**
   * Moves the NXT robot backward until stop() is called.
   */
  public void backward() {
    _updated = false;
    _pilot.resetTachoCount();
    _right0 = 0;
    _left0 = 0;
    _pilot.backward();
  }

  /**
   * Halts the NXT robot and calculates new x, y coordinates.
   */
  public void stop() {
    _pilot.stop();
    updatePosition();
  }

  /**
   *returns true iff the robot is moving under power
   */
  public boolean isMoving() {
    return _pilot.isMoving();
  }

  /**
   * Moves the NXT robot a specific distance. A positive value moves it forwards and a negative
   * value moves it backwards. The robot position is updated atomatically when the method returns.
   * 
   * @param distance
   *          The positive or negative distance to move the robot, same units as _wheelDiameter
   */
  public void travel(double distance) {
    travel(distance, false);
  }

  /**
   * Moves the NXT robot a specific distance. A positive value moves it forwards and a negative
   * value moves it backwards. If immediateReturnis true, method returns immidiately and your code
   * MUST call updatePostion() when the robot has stopped. Otherwise, the robot position is lost.
   * 
   * @param distance
   *          The positive or negative distance to move the robot, same units as _wheelDiameter
   * @param immediateReturn
   *          iff true, the method returns immediately, in which case the programmer <br>
   *          is responsible for calling updatePosition() before the robot moves again.
   */
  public void travel(double distance, boolean immediateReturn) {
    _updated = false;
    _pilot.resetTachoCount();
    _right0 = 0;
    _left0 = 0;
    _pilot.travel(distance, immediateReturn);
    if (!immediateReturn)
      updatePosition();
  }

  /**
   *Rotates the NXT to the left (increasing angle) until stop() is called;
   */
  public void rotateLeft() {
    _updated = false;
    _pilot.resetTachoCount();
    _right0 = 0;
    _left0 = 0;
    _pilot.steer(200);
  }

  /**
   *Rotates the NXT to the right (decreasing angle) until stop() is called;
   */
  public void rotateRight() {
    _updated = false;
    _pilot.resetTachoCount();
    _right0 = 0;
    _left0 = 0;
    _pilot.steer(-200);
  }

  /**
   * Rotates the NXT robot a specific number of degrees in a direction (+ or -).
   * 
   * @param angle
   *          Angle to rotate in degrees. A positive value rotates left, a negative value right.
   **/
  public void rotate(double angle) {
    rotate(angle, false);
  }

  /**
   * Rotates the NXT robot a specific number of degrees in a direction (+ or -). If immediateReturn
   * is true, method returns immidiately and your code MUST call updatePostion() when the robot has
   * stopped. Otherwise, the robot position is lost.
   * 
   * @param angle
   *          Angle to rotate in degrees. A positive value rotates left, a negative value right.
   * @param immediateReturn
   *          iff true, the method returns immediately, in which case the programmer <br>
   *          is responsible for calling updatePosition() before the robot moves again.
   */
  public void rotate(double angle, boolean immediateReturn) {
    _updated = false;
    int turnAngle = (int) Math.round(normalize(angle));
    _pilot.resetTachoCount();
    _right0 = 0;
    _left0 = 0;
    _pilot.rotate(turnAngle, immediateReturn);
    if (!immediateReturn)
      updatePosition();
  }

  /**
   * Rotates the NXT robot to point in a specific direction. It will take the shortest path
   * necessary to point to the desired angle.
   * 
   * @param angle
   *          The angle to rotate to, in degrees.
   */
  public void rotateTo(double angle) {
    rotateTo(angle, false);
  }

  /**
   * Rotates the NXT robot to point in a specific direction. It will take the shortest path
   * necessary to point to the desired angle. If immediateReturnis true, method returns immidiately
   * and your code MUST call updatePostion() when the robot has stopped. Otherwise, the robot
   * position is lost.
   * 
   * @param angle
   *          The angle to rotate to, in degrees.
   * @param immediateReturn
   *          iff true, method returns immediately and the programmer is responsible for calling
   *          updatePosition() before the robot moves again.
   */
  public void rotateTo(double angle, boolean immediateReturn) {
    double turnAngle = normalize(angle - _heading);
    rotate(turnAngle, immediateReturn);
  }

  /**
   * Rotates the NXT robot towards the target point (x,y) and moves the required distance. Method
   * returns when target point is reached, and the robot position is updated;
   * 
   * @param x
   *          The x coordinate to move to.
   * @param y
   *          The y coordinate to move to.
   */
  public void goTo(double x, double y) {
    rotateTo(angleTo(x, y));
    travel(distanceTo(x, y));
  }

  /**
   * Rotates the NXT robot towards the target point (x,y) and moves the required distance. Method
   * returns when target point is reached, and the robot position is updated;
   * 
   * @param x
   *          The x coordinate to move to.
   * @param y
   *          The y coordinate to move to.
   * @param immediateReturn
   *          iff true, method returns immediately
   */
  public void goTo(double x, double y, boolean immediateReturn) {
    rotateTo(angleTo(x, y));
    travel(distanceTo(x, y), immediateReturn);
  }

  /**
   * distance from robot to the point with coordinates (x,y) .
   * 
   * @param x
   *          coordinate of the point
   * @param y
   *          coordinate of the point
   * @return the distance from the robot current location to the point
   */
  public double distanceTo(double x, double y) {
    double dx = x - _x;
    double dy = y - _y;
    // use hypotenuse formula
    return (double) Math.sqrt(dx * dx + dy * dy);
  }

  /**
   * returns the direction angle (degrees) to point with coordinates (x,y)
   * 
   * @param x
   *          coordinate of the point
   * @param y
   *          coordinate of the point
   * @return the direction angle to the point (x,y) from the NXT. Rotate to this angle to head
   *         toward it.
   */
  public double angleTo(double x, double y) {
    double dx = x - _x;
    double dy = y - _y;
    return (double) Math.toDegrees(Math.atan2(dy, dx));
  }

  /**
   * Updates robot location (x,y) and direction angle. Called by stop, and movement commands that
   * terminate when complete. If you use a movement command that returns immediately, you MUST call
   * this method when the movement is complete. It may also be called while movement is on progress.
   */
  public void updatePosition() {
    int temp = 0;
    int left = _pilot.getLeftCount();// left wheel rotation angle
    int right = _pilot.getRightCount();
    temp = left;
    left -= _left0;
    _left0 = temp;
    temp = right;
    right -= _right0;
    _right0 = temp;
    if (left == 0 && right == 0)
      return; // no movement
    int outside = 0;
    int inside = 0;
    byte direction = 0;
    double dx = 0;
    double dy = 0;
    if (Math.abs(left) < Math.abs(right)) {
      outside = right;
      inside = left;
      direction = 1;
    } else {
      outside = left;
      inside = right;
      direction = -1; // turn to right
    }

    double turnAngle =
        direction * (outside - inside) * _pilot._wheelDiameter / (2 * _pilot._trackWidth);

    boolean approx = false;
    if (1.0f * inside / outside > .98) // probably movement was intended to be straight
    {
      double distance = 0.5f * (inside + outside) / _pilot._degPerDistance;
      double projection = (double) Math.toRadians(_heading + turnAngle / 2);
      dx = distance * (double) Math.cos(projection);
      dy = distance * (double) Math.sin(projection);
      approx = true;
    } else {
      double turnRadius = _pilot._trackWidth * (outside + inside) / (2 * (outside - inside));
      double headingRad = (Math.toRadians(_heading));
      if (direction == -1)
        headingRad += Math.PI;
      double turnRad = Math.toRadians(turnAngle);
      dx = turnRadius * (double) (Math.sin(headingRad + turnRad) - Math.sin(headingRad));
      dy = turnRadius * (double) (Math.cos(headingRad) - Math.cos(headingRad + turnRad));
    }
    _heading = normalize(_heading + turnAngle); // keep angle between -180 and 180
    _x += dx;
    _y += dy;
    if (approx)
      _heading -= turnAngle / 2; // correct approximation
    _updated = true;
  }

  /**
   * Moves the NXT robot in a circular path with a specified radius. <br>
   * The center of the turning circle is on the right side of the robot iff parameter radius is
   * negative; <br>
   * Postcondition: motor speed is NOT restored to previous value;
   * 
   * @param radius
   *          is the radius of the circular path. If positive, the left wheel is on the inside of
   *          the turn. If negative, the left wheel is on the outside.
   */
  public void turn(double radius) {
    _updated = false;
    _pilot.resetTachoCount();
    _right0 = 0;
    _left0 = 0;
    _pilot.steer(turnRate(radius));
  }

  /**
   * Moves the NXT robot in a circular path through a specific angle; If waitForCompletion is true,
   * returns when angle is reached. <br>
   * The center of the turning circle is on the right side of the robot iff parameter radius is
   * negative. Robot will stop when total rotation equals angle. If angle is negative, robot will
   * move travel backwards.
   * 
   * @param radius
   *          radius of the turning circle
   * @param angle
   *          the angle by which the robot heading changes, + or -
   */
  public void turn(double radius, int angle) {
    turn(radius, angle, false);
  }

  /**
   * Moves the NXT robot in a circular path through a specific angle; If waitForCompletion is true,
   * returns when angle is reached. <br>
   * The center of the turning circle is on the right side of the robot iff parameter radius is
   * negative. Robot will stop when total rotation equals angle. If angle is negative, robot will
   * travel backwards.
   * 
   * @param radius
   *          see turn(turnRage, angle)
   * @param immediateReturn
   *          iff true, the method returns immediately, in which case the programmer <br>
   *          is responsible for calling updatePosition() before the robot moves again.
   */
  public void turn(double radius, int angle, boolean immediateReturn) {
    _updated = false;
    _pilot.resetTachoCount();
    _right0 = 0;
    _left0 = 0;
    _pilot.steer(turnRate(radius), angle, immediateReturn);
    if (!immediateReturn)
      updatePosition();
  }

  /**
   * returns equivalent angle between -180 and +180
   */
  private double normalize(double angle) {
    double a = angle;
    while (a > 180)
      a -= 360;
    while (a < -180)
      a += 360;
    return a;
  }

  private int turnRate(double radius) {
    int direction = 1;
    if (radius < 0) {
      direction = -1;
      radius = -radius;
    }
    double ratio = (2 * radius - _pilot._trackWidth) / (2 * radius + _pilot._trackWidth);
    return (int) Math.round(direction * 100 * (1 - ratio));
  }
}